//
// Created by Zemon on 2024/11/3.
//

#include "pitch.h"
#include "remote_control.h"
#include "motor_pid.h"
#include "CAN_receive.h"

extern RC_ctrl_t rc_ctrl;
extern motor_measure_t motor_chassis[5];

float set_speed_temp_p;
float set_speed_p;
float set_angle_p;
float kps_p = 20.0f,kis_p = 1.0f,kds_p = 0.0f;
float kpa_p = 0.8f,kia_p = 0.0f,kda_p = 0.0f;
static int16_t lock_flag;

static int16_t angle_flag = 0;
int16_t speed_delta_p;
int16_t max_speed_change_p = 1000;
pid_t pid_speed_p;
pid_t pid_angle_p;

void pitch_init(void){
    angle_flag += 1;
    PID_struct_init(&pid_speed_p,POSITION_PID,16384,16384,kps_p,kis_p,kds_p);
    PID_struct_init(&pid_angle_p,POSITION_PID,2000,175,kpa_p,kia_p,kda_p);

    pid_calc(&pid_angle_p,(float)motor_chassis[4].total_angle,800);
    set_speed_p = pid_angle_p.pos_out;
    speed_delta_p = (int16_t)set_speed_p -motor_chassis[4].speed_rpm;
    if(speed_delta_p>max_speed_change_p)
        set_speed_temp_p = (float)(motor_chassis[4].speed_rpm+max_speed_change_p);
    else if(speed_delta_p<-max_speed_change_p)
        set_speed_temp_p = (float)(motor_chassis[4].speed_rpm-max_speed_change_p);
    else
        set_speed_temp_p = set_speed_p;
    pid_calc(&pid_speed_p,(float)motor_chassis[4].speed_rpm,set_speed_temp_p);

    CAN_cmd_pitch((int16_t)(pid_speed_p.pos_out));
}

void pitch_control(void){

    if(rc_ctrl.rc.ch[1]!=0){
        lock_flag=0;
    }
    else {
        lock_flag=1;
    }

    PID_struct_init(&pid_speed_p,POSITION_PID,16384,16384,kps_p,kis_p,kds_p);
    PID_struct_init(&pid_angle_p,POSITION_PID,2000,175,kpa_p,kia_p,kda_p);

    if(lock_flag==0){
        set_angle_p = motor_chassis[4].total_angle + rc_ctrl.rc.ch[1]*5;

        if(set_angle_p<-1500)
            set_angle_p = -1500;
        else if(set_angle_p>1500)
            set_angle_p = 1500;
        else
            set_angle_p = set_angle_p;

        set_speed_p = -rc_ctrl.rc.ch[1]/10;
        speed_delta_p = set_speed_p - motor_chassis[4].speed_rpm;
        if(speed_delta_p>max_speed_change_p)
            set_speed_temp_p = (float)(motor_chassis[4].speed_rpm+max_speed_change_p);
        else if(speed_delta_p<-max_speed_change_p)
            set_speed_temp_p = (float)(motor_chassis[4].speed_rpm-max_speed_change_p);
        else
            set_speed_temp_p = set_speed_p;
        pid_calc(&pid_speed_p,(float)motor_chassis[4].speed_rpm,set_speed_temp_p);

        CAN_cmd_pitch((int16_t)pid_speed_p.pos_out);
    }

   else if(lock_flag==1){
        pid_calc(&pid_angle_p,(float)motor_chassis[4].total_angle,set_angle_p);
        set_speed_p = pid_angle_p.pos_out;
        speed_delta_p = (int16_t)set_speed_p -motor_chassis[4].speed_rpm;
        if(speed_delta_p>max_speed_change_p)
            set_speed_temp_p = (float)(motor_chassis[4].speed_rpm+max_speed_change_p);
        else if(speed_delta_p<-max_speed_change_p)
            set_speed_temp_p = (float)(motor_chassis[4].speed_rpm-max_speed_change_p);
        else
            set_speed_temp_p = set_speed_p;
        pid_calc(&pid_speed_p,(float)motor_chassis[4].speed_rpm,set_speed_temp_p);

        CAN_cmd_pitch((int16_t)(pid_speed_p.pos_out));
    }




}